Methods and apparati for predicting and quantifying threat being experienced by a modeled system

ABSTRACT

A method predicts and quantifies the threat posed to a human-operated device based on an optimal device trajectory through a constraint-bounded corridor. A model of the device together with a model of anticipated hazards and the current state of both the device and the hazards are used to iteratively generate an optimal device trajectory through a constraint-bounded corridor or region within state space. Device dynamics are forward-simulated over a time horizon. A method generates a threat assessment metric from the resulting sequence of optimal vehicle states. This threat assessment may be used to devise various types and levels of operator assistance. The human operator can control the device within a safe corridor or region. Threat assessment is based on the nearness of successive optimal trajectory predictions to limits of safe device handling rather than on deviation from a predefined path.

RELATED DOCUMENT

This application is the U.S. National Stage of International ApplicationNo. PCT/US2010/042201, filed on Jul. 15, 2010, published in English.Priority is hereby claimed to: U.S. Provisional application No.61/270,933, entitled SEMI-AUTONOMOUS CONTROL OF PASSENGER VEHICLES, inthe names of Sterling J. Anderson, Steven C. Peters and Karl D.Iagnemma, filed on Jul. 15, 2009, referred to herein below as theComprehensive Provisional patent application; and this is acontinuation-in-part of, and priority is hereby claimed to U.S. patentapplication Ser. No. 12/711,935, PREDICTIVE SEMI-AUTONOMOUS VEHICLENAVIGATION SYSTEM, in the names of Sterling J. Anderson, Steven C.Peters and Karl D. Iagnemma, filed on Feb. 24, 2010; and this is acontinuation-in-part of, and priority is hereby claimed to InternationalApplication No. PCT/US2010/025206, PREDICTIVE SEMI-AUTONOMOUS VEHICLENAVIGATION SYSTEM, in the names of Sterling J. Anderson, Steven C.Peters and Karl D. Iagnemma, filed on Feb. 24, 2010, designating theUnited States, both of which claim priority to U.S. Provisionalapplication No. 61/209,250, entitled PREDICTIVE SEMI-AUTONOMOUS VEHICLENAVIGATION SYSTEM, in the names of Sterling J. Anderson, Steven C.Peters and Karl D. Iagnemma, filed on Mar. 5, 2009, priority to which isalso claimed herein, referred to below as the Semi-Autonomous patentapplications, which are all hereby fully incorporated herein byreference. This case is also related to a PCT application that was filedon Jul. 15, 2010, in the names of Sterling J. Anderson and Steven C.Peters, and Karl D. Iagnemma, submitted by electronic filing underInternational Application No. PCT/US2010/042203, entitled, AN INTEGRATEDFRAMEWORK FOR VEHICLE OPERATOR ASSISTANCE BASED ON A TRAJECTORYPREDICTION AND THREAT ASSESSMENT, which is hereby fully incorporatedherein by reference, and is referred to herein below as the co-pendingOperator Assistance application.

BACKGROUND

Inventions described herein relate to a novel, corridor-based frameworkthat performs threat assessment and provides varying degrees of mutuallyconsistent automated operator assistance in human-machine systems, suchas locally or remotely-operated passenger vehicles, transport vehicles,agricultural machinery, fork lift trucks, aerial vehicles, robots, orsurgical tools. This framework explicitly considers human and machinedynamics without presuming operator intentions or limiting the avoidancemaneuver (and its associated threat assessment) to a specific path. Itprovides a unified framework that allows for various modes and levels ofmutually consistent operator assistance, from operator warning tostability control to passive intervention, to active semi-autonomouscontrol, and finally, to autonomous machine operation.

Automotive active safety systems are concerned with preventing accidentsthrough the introduction of various computer-controlled actuationmethods to inform, improve, or override a human operator's steeringand/or braking performance. Active safety systems currently in existenceinclude yaw stability control, roll stability control, traction control,and antilock braking, among others. While these systems reduce accidentfrequency, their path-based and largely reactive nature limits theirability to: 1) accurately assess the threat posed by a given scenarioand 2) adequately determine when and how to intervene to assist thedriver. This dependence on a specific trajectory (amidst a myriad ofoptions available to the operator) reduces the accuracy and significanceof the threat assessment and leads to controllers that selectivelyreplace (rather than assist) the driver in order to follow anautomation-designated path.

The inventions described and claimed herein relate, primarily, to threatassessment aspects of this novel framework. Inventions described andclaimed in the Operator Assistance application relate primarily to theunified nature of the framework, and its flexibility in being able toaccurately assess threat, and then participate in one or more of a widevariety of mutually consistent operator assistance modes, of varyinglevels of operator autonomy, from complete autonomy, to no autonomy.

A basic premise of threat assessment for such assisted and automatedsystems is generally as follows. First, sensing systems such as radar,LIDAR, cameras, inertial measurement units and GPS localization systemsare used to detect, classify, and track the position of objects and thedrivable road surface in the host vehicle's vicinity as well as measurevehicle states. Once these potential hazards have been identified,localized and their motion has been estimated, a threat metric is usedto quantify the threat they pose to the host vehicle, together with thethreat of departing the drivable road surface due to loss of vehiclecontrol. As used herein, threat assessment or threat prediction is usedto mean identifying hazards and quantifying threat. Many threatassessment technologies are designed to then trigger and/or implementcountermeasures to reduce the threat. These countermeasures can bepassive or active. The effectiveness of threat assessment metricsdepends on their ability to correctly identify hazards and accuratelyassess the threat that potential hazards pose to the host vehicle.

Threat metrics described in the literature predominantly use time-based,distance-based, and deceleration-based measures to characterize thethreat level of a given scenario. Time-based threat measures projecttime to collision (TTC) based on current speeds, positions,trajectories, and (in some formulations) other vehicle states.Distance-based metrics are generally calculated using prevailing rangeand vehicle speeds and require constant velocity/accelerationassumptions and simple hazard geometry. Finally, acceleration-basedmetrics assess the threat of a given maneuver based on the minimum (andoften assumed constant) lateral or longitudinal acceleration that asimple avoidance maneuver would require, given the current position,velocity, and acceleration of both host and hazard. In another approach,estimate is made of the lateral acceleration required to execute aconstant radius evasive maneuver. That implementation then compares thisacceleration to a threshold value. When the required accelerationreaches this threshold, braking countermeasures are implemented toreduce the vehicle's longitudinal velocity.

While the above threat metrics have been shown to provide usefulestimates of the danger posed by a given maneuver, they suffer from manydrawbacks. They are not well suited to consider multiple hazards,complex vehicle dynamics, or complicated environmental geometry with itsattendant constraints. The geometrically-simple (straight-line orconstant-radius-turn (CRT)) avoidance maneuvers assumed by these metricsmay also misestimate the true threat posed by scenarios where theoptimal avoidance trajectory follows a curve of varying radius ornon-constant velocity/acceleration.

At least one known method of assessing threat relates to a vehicle thatis intended to navigate along a path. The path may be predetermined, orcalculated, based on data, such as information about obstacles and apath followed by a track, such as a roadway in the case of a roadvehicle, such as an automobile. The path is a curve of simple geometry,having essentially no width. If the vehicle deviates from the zero widthpath, the system determines that danger has arisen, and the systemgenerates a threat signal. However, in fact, the threat of actual dangeris potentially low, because vehicles driven by a human operatortypically operate within a field of safe travel, or a corridor, ratherthan along a relatively arbitrary line, such as the centerline of aroadway. This dependence on a specific trajectory (amidst a myriad ofoptions available to the operator) reduces the accuracy and significanceof the threat assessment and leads to controllers that selectivelyreplace (rather than assist) the driver in order to follow anautomation-designated path.

The following terms will be used herein as follows. A path is a simplegeometric curve in two-dimensional space, along which a vehicle maytravel. The path, in x-y space may be defined by a function, y=f(x). Apath has a width of essentially zero. A trajectory is aphysically-achievable and time-parameterized sequence of vehicle states(such as velocity, yaw angle, wheel sideslip angle, etc.) over a timehorizon, By physically-achievable, it is meant that for everytrajectory, there exists a set of controller inputs, such as brakingtorque and steering angle, that when applied to a model of the vehicleproduce the desired trajectory. It has been mentioned that thetrajectory includes the velocity of a vehicle as an element. It may alsobe thought of as having time as a parameter of the path, which may thenestablish velocity at different locations.

A corridor is a swath through two-dimensional space, which may bedefined by an inequality y_(min)<y<y_(max), where each of y_(min) andy_(max) are themselves defined by y_(max)=g(x) and y_(min)=h(x). Thus, acorridor may be considered to be the space between two curves intwo-dimensional space. Travel anywhere within the corridor is consideredto be safe. A region is a concept that is defined in connection withinventions hereof, and it will be defined below.

A Model Predictive Controller (MPC) is an optimal control methodtypically used to generate an optimal set of control inputs (spanningthrough a future time horizon) required to track a desired path whileminimizing a user-defined objective function. In typical usage, only thefirst element of this command input sequence is implemented at eachsolution timestep and the remainder are disregarded.

A human driver typically operates a vehicle within a safe range ofvehicle states. For example, the driver typically maintains thevehicle's position on the roadway (lateral position state) within thecorridor defined by road or lane edges. Similarly, other states such asvehicle velocity, lateral acceleration, etc. provide some indication ofthreat to the driver, who (consciously or subconsciously) seeks to keepthem within a safe operating range (or between upper and lower bounds).Depending on the driving conditions, posted speed limits, and otherfactors, for example, a driver may allow vehicle velocity to varybetween 53 and 70 miles per hour rather than slavishly maintaining 55miles per hour along a roadway. Likewise with vehicle sideslip, whichthe driver will typically strive to maintain within a reasonable (orsafe) range. Thus, a human operator operates within an N-dimensionalregion of state space, rather than along a simple, zero, or nearly-zerowidth curve of a physical trajectory.

In many cases, it may be desirable to assess threat based on a realisticroadway corridor than an unrealistic single path on the roadway.Similarly, it may also be desirable to assess threat based on arealistic N-dimensional region of state space (as explained more fullybelow, which includes as a portion, the two-dimensional corridor).

Some known threat assessors exist as a separate system, not integratedwith other systems of the device analysis and control apparatus. Thesesystems may base the assessment of threat on a device state exceeding arelatively arbitrary threshold. Further, these threat assessorstypically do not provide the control input necessary to decrease thethreat they've assessed.

Other threat assessment approaches use only rudimentary threatassessment metrics based on, for example, the current deviation of thedevice from a predetermined optimal path. It would be desirable to beable to take advantage of predicted future states of the device inassessing threat.

Still other threat assessment approaches also do not consider combinedeffects of vehicle dynamics, stability constraints and terraininteractions to evaluate maneuver severity. However, it would bedesirable to be able to consider these matters in threat assessment.

Thus, an object of inventions hereof is to take advantage of predictedfuture states and predicted future optimal inputs to assess threat. Afurther object would be to be able to consider combined effects ofvehicle dynamics, stability constraints and terrain interactions toevaluate maneuver severity in assessing threat. Still another objectwould be to be able to assess threat on a realistic corridor, ratherthan an unrealistic single path. Another object would be to assessthreat with an apparatus that is integrated with other systems of systemanalysis and control, which are also used to control the device orsystem, rather than assessing threat on a device state exceeding anarbitrary threshold. Still another object would be to use the assessmentof threat to generate an operator assistance signal to assist theoperator in safely operating the device or replace the operator asnecessary to ensure safe operation of the device.

These and other objects of inventions disclosed herein will be morefully explained and understood with reference to the Figures of theDrawing, which are:

BRIEF DESCRIPTION OF THE FIGURES OF THE DRAWING

FIG. 1 is a block diagram illustrating basic framework operation;

FIG. 2 graphically shows an example of various potential interventionlaws based on threat metric calculation;

FIGS. 3 a and 3 b graphically show an obstacle avoidance scenarioillustrating different stages of intervention for an inattentive driver,with FIG. 3 a showing the different levels of intervention, and FIG. 3 bshowing the relative locations of the host vehicle and the environment;

FIG. 4 shows, in flowchart form, basic steps of a method of threatassessment and semi-autonomous control, with possible considerations ateach step;

FIGS. 5 a, 5 b and 5 c show, graphically, a simulated test illustratingsystem response when driver fails to navigate a curve in the road, asshown schematically in FIG. 5 a, with steering angle shown in FIG. 5 band control authority K shown in FIG. 5 c, where K represents theproportion of control authority given to autonomous system, with thedriver allowed the remaining (1−K);

FIGS. 6 a, 6 b and 6 c show, graphically, a simulated test illustratingsystem response to an erroneous driver swerve, as shown schematically inFIG. 6 a, with steering angle shown in FIG. 6 b and control authority Kshown in FIG. 6 c, where K represents the proportion of controlauthority given to autonomous system, with the driver allowed theremaining (1−K);

FIGS. 7 a, 7 b and 7 c show, graphically, a simulated test illustratingsystem response when driver fails to anticipate/avoid obstacle, as shownschematically in FIG. 7 a, with steering angle shown in FIG. 7 b andcontrol authority K shown in FIG. 7 c, where K represents the proportionof control authority given to autonomous system, with the driver allowedthe remaining (1−K);

FIG. 8 shows, in flow chart form, steps of a method of an inventionhereof for assessing threat, with additional considerations associatedwith some of the steps;

FIG. 9 a shows, schematically, an illustration of a simple path trackingcontrol set up of the prior art;

FIG. 9 b shows, schematically, a corridor keeping control set up, usingposition constraint y=f(x) for static hazards;

FIG. 10 is a schematic representation in block diagram form illustratinghow a representative system of the prior art that can lead to poor,mutually inconsistent performance;

FIG. 11 is a schematic representation in block diagram form showing anarray of functions performed in a unified, mutually consistent manner byinventions hereof.

DETAILED DESCRIPTION

Inventions described in the above-referenced Semi-Autonomousapplications, and the Comprehensive Provisional patent application,relate, among other things, to a unified framework for performing threatassessment and semi-autonomous vehicle navigation and control whileallowing for adaptable and configurable intervention laws andconfigurable control inputs.

Inventions described herein relate to methods and apparati foridentifying and quantifying threat being experienced by a system thatcan be modeled, such as a vehicle, such as a road vehicle, such as anautomobile.

To summarize briefly inventions claimed herein, a method predicts andquantifies the threat posed to a human-operated device based on anoptimal device trajectory through a constraint-bounded corridor. A modelof the device together with a model of anticipated hazards and thecurrent state of both the device and the hazards are used to iterativelygenerate an optimal device trajectory through a constraint-boundedcorridor or region within state space. Device dynamics areforward-simulated over a time horizon. A method of an invention hereofgenerates a threat assessment metric from the resulting sequence ofoptimal vehicle states. This threat assessment may be used to devisevarious types and levels of operator assistance. The human operator cancontrol the device within a safe corridor or region. Threat assessmentis based on the nearness of successive optimal trajectory predictions tolimits of safe device handling rather than on deviation from apredefined path.

As used herein, identifying and quantifying threat will be referred toas assessing threat, or sometimes, predicting threat. Danger may also beused interchangeably with threat. The methods and apparati for assessingthreat are of the same type as can be used in the unified framework forsemi-autonomous vehicle navigation and control, discussed in theapplications mentioned above. Such methods and apparati can be used forassessing threat in any system that can be modeled.

In contrast to existing approaches, the inventions described herein donot rely on a subjective prediction of a path intended by a humanoperator or a specific path proposed by an automatic path planningalgorithm. Instead, these inventions base threat assessment and operatorassistance decisions on dynamic properties and known constraintsinherent to the vehicle and the environment. These inventions warn ofdeviation from a physically-constrained and dynamically-feasible regionin N space (that includes a two-dimensional corridor in physical space)rather than an arbitrarily-calculated path, which is often lessmeaningful since it often does not represent a true, or even achievable,much less optimal route. Further, by predictively simulating the vehicledynamics over a time horizon, which may be finite or infinite, itsthreat assessment and intervention operations explicitly andpre-emptively consider the combined effects of vehicle dynamics,stability constraints, and terrain interaction on maneuver severity.

Turning again to definitions of some helpful terminology, a region is anN-dimensional analogue of a corridor. A region is an n-dimensional areadefined in the state space of the device model over a time horizon. Thisregion is bounded by corridor constraints (which apply to vehicleposition states), together with other state constraints (such as thoseimposed on vehicle states such as yaw angle, yaw rate, velocity, wheelsideslip angle, etc.).

In contrast to known MPC planners, MPC planners of inventions hereof arenot required to utilize a single reference path for optimal controlinput generation. Rather, the MPC planners of inventions hereof arecapable of generating an optimal trajectory that constitutes a paththrough a constraint-bounded corridor or region of the state space.Thus, planners described herein generate a course of motion that neednot follow a predefined path but may instead generate its own optimaltrajectory (and the control inputs necessary to achieve it) atsuccessive time steps.

Threat assessment inventions described herein may be used with anysystem that can be modeled, including vehicles, such as terrestrial,nautical, and aerial vehicles, manufacturing machines, chemicalprocesses, and fully automated processes, such as nuclear power plantoperations and economic processes. All such systems and devices that canbe reasonably modeled, can have associated therewith conditions thatthreaten operator, client, or third party use and enjoyment or processeffectiveness. As such, there is benefit in assessing these threateningconditions, and issuing warnings therefore, possibly enabling countermeasures or other preparations. The specific implementations describedherein are illustrated in the context of a terrestrial vehicle, such asan automobile. However, it is to be understood that the explanation isfor illustrative purposes only, and that the inventions described hereincan be used to identify and quantify (i.e., assess) threat in allsystems that can be modeled. The systems may be physical, such asdevices and chemical processes. They may also be non-physical, such aseconomic systems. In all cases, the system model will include states(such as pitch angle for an aerial vehicle, solvent concentration for achemical system, or asset prices for a financial system) for which adesired value or range of values exists and to which constraints (suchas stall limits for an aerial vehicle, saturation for a chemicalprocess, or price caps for a financial system) may apply.

Turning then to terrestrial systems, for illustration purposes only,automotive active safety systems are concerned with preventing accidentsthrough the introduction of various computer-controlled actuationmethods to improve driver braking and steering performance. Currentactive safety systems include yaw stability control, roll stabilitycontrol, traction control, and antilock braking, among others. Whilethese systems reduce accident frequency, they are fundamentally reactivein nature: their intervention is based on current vehicle (and,possibly, road surface) conditions. Because they do not utilize 1)sensory information related to the vehicle surroundings or 2) aprediction of the vehicle's path through its surroundings, they havelimited ability to assess the threat of impending accidents, and thuscannot exert corrective actions to avoid them.

Active navigation systems aim to avoid accidents by utilizing sensoryinformation related to the vehicle surroundings and a prediction of asafe vehicle trajectory through those surroundings to exert appropriateactuator effort to avoid impending accidents. Sensory information wouldinclude data related to nearby vehicles, pedestrians, road edges, andother salient features to assess accident threat.

Except in cases of desired full automation, such navigation systemsideally operate only during instances of significant threat: it shouldgive a driver full control of the vehicle in low threat situations butapply appropriate levels of computer-controlled actuator effort duringhigh threat situations. An active navigation system can therefore betermed semi-autonomous, since it must allow for human-controlled,computer-controlled, and shared human/computer vehicle operation. Such asystem should be as unobtrusive to the driver as possible (i.e. itshould intervene only as much as is minimally required to avoid animpending accident).

A semi-autonomous active navigation system described in part in theabove referenced Semi-Autonomous patent applications can satisfy theabove requirements and desired characteristics. Further, it provides aframework into which various distinct sensing and actuation modes can beeasily incorporated. The system's method for threat assessment andcomputer-controlled intervention can potentially be modified in realtime based on the scenario, environmental conditions, driver preference,or past driver performance. FIG. 1 shows, schematically, in blockdiagram form, a basic framework operation.

A model Predictive Control (MPC) vehicle navigation block 110 startswith a model 112 of the environment, a model 114 of the device, in thiscase, a vehicle, and the vehicle's current state, including theposition. It generates an optimal vehicle trajectory from the currentposition through a time horizon. The trajectory is optimal with respectto a pre-defined, configurable set of criteria. It also generates acorresponding optimal set of control input commands necessary to executean optimal trajectory within the corridor and ensure that the vehicle102 operates within safe driving limits (defined by a constraint-boundedregion in the state space). The environment model can be based on apriori known information (e.g. from maps) and/or information gathered byreal time sensors, such as on-vehicle sensors 104 (e.g. cameras andlaser range-finders, vehicle to vehicle (V2V sensors), and can includeinformation 106 related to the environmental and potential environmentalhazards, such as position of road edges, lane boundaries, holes, slopes,static obstacles (e.g. trees, road-side signs), and dynamic obstacles(e.g. other vehicles, pedestrians). The vehicle model is user-definedand can be of varying complexity and fidelity. The real-time sensors mayalso be mounted in the environment and communicate with the controlsystem on the vehicle.

The predicted safe vehicle trajectory (and associated control inputs toyield such a trajectory) is generated such that it satisfies aconfigurable set of requirements, including, for example, that thevehicle position remain within a safe driving corridor of the roadway,that the vehicle sideslip angle not exceed the safe limit of vehiclehandling, that tire friction forces not exceed a surfacefriction-limited value, and others. Note that by permitting thetrajectory to satisfy locations within a corridor, rather than the morestringent, yet more arbitrary, locations along a (essentially zerowidth) path, the controller avoids restricting its solution to oneparticular path and instead iteratively calculates the optimaltrajectory within the corridor at every sampling time step. The controlinputs can be associated with one or multiple actuators, such as activesteering, active braking, and others. The predicted vehicle trajectoryand associated control inputs may be generated via constrained optimalcontrol, which leverages efficient optimization methods andconstraint-handling capabilities. In particular, model predictivecontrol techniques may be used to generate a set of optimal trajectoriesand associated control inputs, etc. It should be understood thattrajectory is used herein to mean a sequence of vehicle states,including its position, velocity, sideslip and yaw angles, etc.

At successive discrete sampling instants, the predicted vehicletrajectory and predicted control inputs are analyzed by a threatassessor 108 to quantify the threat to the vehicle by computing aconfigurable metric, such as the maximum lateral acceleration, sideslipangle, or roll angle over the trajectory, the minimum proximity toobstacles, or other metrics. Generation and use of this threatassessment metric is discussed in more detail below, and is a focus ofinventions described herein. Threat may be more formally considered tobe a hierarchical combination of obstacle avoidance, stability-criticalstates, inputs, etc, based on a model of the vehicle.

In the case of a semi-autonomous system, the control authority exertedby the system can then be determined as a function of this generatedthreat: generally speaking, if the threat metric value is low, thecontrol system intervention is low (i.e. the driver commands the vehiclewith little or no computer-controlled intervention); if the threatmetric value is high, the control system intervention is high. The formof the intervention law modulating this control system authority isconfigurable and can differ for different actuators (i.e. a vehicle withboth active steering and braking can have distinct intervention lawsdefined for the steering actuator and the braking actuators). Theintervention law can also be defined to adapt to driver performancebased on an assessment of driver skill, and/or to include considerationsfor driver preference, environmental conditions, previous threat metricvalues, previous control inputs, and other factors. FIG. 2 shows,schematically, examples of various potential intervention laws, showing,from top to bottom, linear, smooth and threshold-shaped interventionlaws that depend only on predicted threat. The vertical axis representsthe degree of control authority given to the active navigation andcontrol system while the horizontal axis represents the predictedthreat, with cause for intervention increasing from left to right.

In the system described above, as the threat metric value increases,indicating that the predicted vehicle trajectory will near a pre-definedcritical vehicle state(s) (such as spatial location, lateralacceleration, or tire friction saturation), the control system begins toassume control authority to preempt an unsafe maneuver. As the threatmetric decreases, the controller's authority phases out. In this manner,the system can be said to be both predictive and semi-autonomous.

Note that in extreme cases, when the driver does not perform anappropriate corrective action, it is conceivable that a required hazardavoidance maneuver will reach vehicle handling limits. To account forsuch scenarios, the intervention law can be designed such that itassumes full authority by the time the predicted safe trajectory reachesthe limit of any pre-defined critical vehicle states. This correspondsto a situation where only an optimal set of inputs would result in asafe vehicle trajectory.

FIG. 3 shows schematically an obstacle avoidance scenario illustratingdifferent stages of intervention for an inattentive driver. Initially,the host automobile 302 is at location 1. No obstacles are in view, andthe optimal predicted trajectory is a straight path. The predictedthreat is at a low level, indicated by the vertical line designated 1,near to the left hand side, which represents low threat. As the vehicle302 advances along the roadway to the location 2, it comes nearer to atruck 304, whose velocity is either zero, or much less than that of thehost vehicle 302. The sensors sense the proximity of the obstaclevehicle 304, and generate a threat metric that is larger, as at thevertical line designated 2, near the right hand limit of the threatscale. The optimal predicted trajectory assumes a curved shape, to avoidthe obstacle 304. Simultaneously, the level of intervention K, increases(as shown by each of the three different intervention laws) so that, ina semi-autonomous system, the controller would take more and morecontrol, the nearer to the obstacle the host vehicle 302 comes. In othersystems, the controller may take different countermeasures, such asinitiating a warning, priming brakes, seatbelts, or airbags, and/orengaging active systems, etc.

FIG. 4 shows, schematically, in flow chart form, a basic flow of stepsperformed by a controller of an invention described in theSemi-Autonomous applications, above, with possible considerations ateach step.

An initial step 402 generates an optimal set of control inputs andcorresponding vehicle trajectory by forward simulation. Considerations402 a for this step include, for example, (but are not limited to) thevehicle dynamics, current state of the vehicle and environment, terrainand environmental disturbances, available actuation, trajectoryobjectives, safety limits, and driver inputs.

A next step is to assess 404 the predicted threat to the vehicle.General considerations 404 a for this step include characteristics ofthe optimal path and associated control input, safety limits and driverinputs. The method of threat assessment is discussed below in moredetail in connection with FIG. 8.

Returning to a brief discussion of a semi-autonomous system, a next stepis to generate D06 control authority gains, with a major considerationD06 a at this stage being the desired intervention characteristic. Thenext step D08 is to implement the scaled control for the current time.

Simulation studies have been conducted. They are presented here to showhow the threat assessment metric might be used to govern the level ofassistance provided to a human operator. FIGS. 5 a, 5 b and 5 c show,graphically, the results of a simulated test illustrating systemresponse when a driver fails to navigate a curve in the road, shown byin FIG. 5 a by a pair of lines. The trajectory that the driver wouldhave followed without assistance is shown dashed. Note that it leavesthe roadway. With assistance, it is shown solid black and remains withinthe roadway. Note that here, K represents proportion of controlauthority given to the autonomous system, with the driver allowed theremaining (1−K). The middle graph, FIG. 5 b shows the steer inputs, withthe dashed line corresponding to the driver and the solid curvecorresponding to the control system. The lower graph, FIG. 5 c, showsthe control authority given to the autonomous system, in this case,steering, with the degree varying with distance (x) along the horizontalscale.

FIGS. 6 a, 6 b and 6 c show, graphically, the results of a simulatedtest illustrating the system response to an erroneous driver swerve.Again, K represents proportion of control authority given to autonomoussystem, with the driver allowed the remaining (1−K). The same line typesas above correspond to the driver without assistance (gray dashed) andwith assistance (solid line). The safe roadway corridor is shown in FIG.5 a by a pair of light solid lines in the upper graph. Distance is shownalong the horizontal scale. The assisted trajectory remains within thesafe roadway.

FIGS. 7 a, 7 b and 7 c show, graphically, a simulated test illustratingsystem response when a driver fails to anticipate/avoid an obstacle,similar to the scenario illustrated above with respect to FIGS. 3 a and3 b. Again, K represents the proportion of control authority given toautonomous system. The obstacle is simulated by a jog in the light linethat represents one edge of the safe roadway. The only inputs used inthis simulation are, again, steering of the driver and the autonomoussystem.

Significant advantages stem from the predictive nature of this solution.In addition to considering past and current vehicle and driver actionsto assess threat and determine control authority, the solution generatedby the present inventions predicts a future vehicle trajectory andassociated threat, and uses this prediction to schedule controlauthority.

This predictive nature also allows for a more accurate assessment ofthreat than is otherwise possible. While other threat assessment metricsrely largely on physics-based calculations, the metrics used in theinventions disclosed herein can derive from sophisticated physics basedvehicle and environmental models. These models yield very accuratethreat assessments by considering the effects of terrain conditions,environmental disturbances, and physical limitations of vehicleactuators. These models can also assess threat for more complex vehicletrajectories than is possible with simplified models.

Threat assessment techniques disclosed herein, and uses within a systemas described herein provide improved modularity and adaptability whencompared to previous methods and apparati. Aspects of this improvedmodularity are discussed in more detail below. The underlying controlframework can accommodate multiple actuation modes and vehicle models,allowing for ready application of the system to various vehicle typesand actuator configurations. The system's intervention law is alsoreadily adapted (i.e. it can change over time based on an assessment ofdriver skill, driver preference, environmental conditions, previousthreat metric values, previous control inputs, and other factors). Theseadaptations can be performed either statically or dynamically.

DETAILED DISCUSSION OF THREAT ASSESSMENT

Turning now to a more detailed discussion of threat assessmenttechniques, first, the methods will be described generally, with the aidof a block diagram, and then a more detailed mathematical basis will beprovided.

Gauging threat, using metrics based on predicted vehicle state evolutionwithin a region/corridor is novel. This includes threat assessment basedon a trajectory that remains within that corridor, along withsemi-autonomous control necessary to keep the vehicle within the safecorridor. Various threat assessment methods exist. Some are based on apredicted and/or optimal vehicle trajectory. However, none use a regionor corridor of safe travel to characterize the trajectory. By corridor,it is generally meant a portion of physical space, such as the width ofa roadway, or a roadway and adjoining break down lane. By region, it isgenerally meant a region in N-space, in which two of these N dimensionsmay be the two spatial dimensions of the roadway's width and length, andother dimensions may be states and/or control inputs of the vehicle,such as sideslip angle, yaw angle, velocity, steering angle, etc. Thisleads to significant difference in performance of the two generalapproaches. The simple path-based approach of the prior art is suitedonly to warn or selectively replace a human operator. Thecorridor/region approach described herein may warn, supplement, or actin conjunction with a human operator, as well as replace.

Turning now to FIG. 8, a representative process for assessing threat isdescribed. The steps about to be described, 802-808, are all conductedat each time step of conducting a model predictive control operation. Anoptimal path is generated 802 through an environment, using modelpredictive control techniques. Considerations 802 a that are taken inconjunction with this step may include, but are not limited to: vehicledynamics and constraints, trajectory objectives (controller objectivefunction), terrain and environmental disturbances and environmentalinformation, such as: obstacle locations, terrain properties (slope,roughness, friction coefficient, etc.) and other disturbances (such aswind). It is through these considerations of vehicle dynamics andconstraints and obstacle locations, that the corridor aspects of thismethod of threat assessment enters. Trajectory objectives more broadlydescribes the penalties applied to violating constraints and/orapproaching unstable states and shapes what an optimal solution lookslike within the corridor.

The model predictive controller generates an optimal trajectory, whichconstitutes an optimal sequence of inputs and the corresponding set ofoptimal vehicle states over a time horizon. These outputs are generatedby the MPC unit, based on analysis of the models of the vehicle and theenvironment, using model predictive control over a time horizon, asdiscussed in more detail below.

It is to be noted that threat assessment methods disclosed hereinoperate on an optimal (sometimes referred to herein and the literatureas best case, or benchmark) trajectory that was generated to remainwithin a constraint bounded, traversable, corridor, while maximizingvehicle stability. Prior art path based methods use a desiredtrajectory. The difference between optimal and desired, is that adesired path gives a rather inflexible and in some cases, arbitrary goalstate to which the driver will be forced to adhere or risk setting offwarning indicators or controller intervention, while the optimaltrajectory gives an indication of the threat posed to the vehicle and abackup plan in case the human operator doesn't keep the vehicle withinthe traversable corridor him/her self.

Consider a situation in which, for example, a driver diverges from alane centerline (the desired path for prior-art systems). While a pathbased system will try and pull the driver back, inventions disclosedherein will re-compute a new trajectory that may be completely satisfiedby staying off the centerline, as long as the vehicle is within thecorridor.

The optimal trajectory, of predicted states and predicted vehiclecontrol inputs, over a time horizon, is next coalesced 804 to generate ascalar prediction metric. Considerations 804 a that contribute to thestep of generating a scalar metric may include but are not limited to:past, present and/or predicted states, inputs and objective functioncosts. Various norms (discussed below) may be used to combine trajectorycomponents into this scalar. Past present and/or predicted operatorinputs and performance may also, but need not be taken into account.

The result of the coalescing step 804 is a scalar prediction metric.This scalar can be non-dimensionalized 806 by normalizing it againstknown or approximated physical limits and/or predetermined desiredthresholds to obtain a threat assessment at the current time. Suitablecandidates for such thresholds include, but are not limited to maximumsideslip angle before loss of control, maximum load transfer beforewheel liftoff, maximum lateral acceleration before skidding, maximumlongitudinal acceleration before skidding, maximum total accelerationbefore skidding, maximum steer angle before actuator saturation, maximumavailable acceleration torque, and maximum available braking torque.

Considerations 806 a that may be taken into account include, but are notlimited to known physical limits on vehicle dynamics (e.g. frictionsaturation limits, rollover thresholds, etc), desired interventioncharacteristics, operator performance and operator preference.

The result of the non-dimensionalizing step 806 is a threat assessment,which can then be used for one or more of a variety of mutuallyconsistent threat response functions, depending upon the systemconfiguration. The threat assessment can be used to generate a warning,which may be perceptible by any human sense, including audible, visual,haptic and olfactory. The threat assessment can also, or alternatively,be used to trigger assistance, which may be passive, or active, asdiscussed above, and, if active, to varying degrees, depending onautonomy considerations. The threat assessment may even be used tomodulate other system or vehicle characteristics.

Examples of passive assistance in the context of vehicular controlinclude, but are not limited to: resistance torques on steering wheel,traction control, anti-lock braking systems. Examples of activeassistance include, but are not limited to adaptive cruise control, yawstability assistance Electronic Stability Control (ESC) lane keepingassistance, obstacle avoidance. Modulation of other systemcharacteristics may include but are not limited to: seatbeltpretensioning, brake priming, suspension modifications (activesuspensions, suspension stiffness modifications, etc.)

Each of the foregoing steps, from the step 802 of generating an optimalpath, through using 808 the threat assessment to initiate a threatresponse, such as to take some action, if need be, is conducted at eachiteration of the Model Predictive Control operation, which repeats at afrequency tailored to the particular process under control. Forinstance, in a representative automobile control system, the MPC routinehas been conducted 20 times per second, (each timestep requiring 50millisecond to compute). Of course, different applications withdifferent hardware and even different vehicle models have drasticallydifferent parameters.

Turning now to more formal considerations, Model Predictive (MPC) (orreceding horizon) Control is a family of finite-horizon optimal controlschemes that iteratively minimizes a performance objective defined for aforward-simulated plant model subject to performance and inputconstraint. Stated another way, MPC uses a model of the plant to predictfuture vehicle state evolution and optimize a set of plant controlinputs such that this prediction satisfies constraints and minimizes auser-defined objective function.

Model predictive control has a number of significant properties thatmake it particularly well suited to threat assessment for use with, forinstance, autonomous and semi-autonomous vehicle navigation problems.Its ability to explicitly consider environmental, performance andactuator constraints enables corridor-based navigation and allows it tooperate near the limits imposed by those constraints. Thisenvironmentally aware prediction, coupled with anobjective-function-optimal control law, has been shown to closely mimicthe performance of a human driver. Some implementations use a finiteprediction horizon, which fits naturally with and may be based on theinformation provided by finite-horizon, forward-looking sensors.Additionally, the model-based nature and multivariable-compatibility ofthe control calculation allows MPC to account for and easily adapt tostructural changes and actuator availability from one vehicle modeland/or loading configuration to the next. This adaptability may allowfor reduced-cost controller implementation across product families andthrough ever-shifting safety requirements.

Finally, MPC's predictive nature allows the innovative use disclosedherein of certain constrained configurations to automatically plan apath within a partitioned environment without requiring any pre-definedvehicle trajectories. That is, where other control methods require aspecific pre-planned path through the environment (which is oftenplanned by a separate and suboptimal system), MPC can be configured toaccording to inventions disclosed herein, to plan its own, optimal, pathgiven a set of situational position constraints. The path thus plannedthrough the (pre-delineated) safe operating environment potentiallyoffers a number of advantages over alternative trajectory planningmethods; not only is it explicitly aware of vehicle dynamics, measureddisturbances, and actuator limitations, but the constraint-satisfyingtrajectory plan it generates is feasible, since it is obtained from analready-generated set of control inputs. It is also optimal, withrespect to some performance metric such as minimum lateral accelerationover a future time horizon, minimum wheel slip, etc. In thesemi-autonomous framework described below, this optimal prediction canserve not only as a optimal trajectory plan, but also as an effectivethreat assessor.

At each time step, t, the current plant state is sampled and acost-minimizing control sequence spanning from time t to the end of acontrol horizon of n sampling intervals, t+nΔt, is computed subject toinequality constraints (which establish the safe travel corridor). Thefirst control element of this input sequence is implemented at thecurrent time and the process is repeated at subsequent time steps. Astate sequence spanning the same time period is also generated. No knownMPC process uses elements of either of these sequences, after it hasidentified the single control input element for the current time. Onlythat single, current control input element is used.

Inventions disclosed herein use these subsequent elements of one or boththe state and control sequences to assess threat to which the device is(or may in the future be) exposed.

The optimal-control-based method that uses sensor information (andcorresponding corridor boundaries) to generate controller inputs alsoenables generation of a metric for analyzing the threat posed to thedevice by a given scenario. This metric is comparable to and in manysituations more useful/accurate than existing metrics because it isbased solely on known or approximated physical limits of the environmentand a (generally very accurate) model of the vehicle.

In contrast to known methods of threat assessment used in the prior art,threat assessment methods and apparati, as disclosed herein, generatinginstantaneous threat from an MPC-derived optimal avoidance trajectoryinherently considers multiple hazards, actuator limitations/effects,measured disturbances, and (using nonlinear MPC), variable vehiclevelocities and accelerations. Configuring the controller to plan a(sideslip-minimizing) trajectory within a safe region of travel ensuresthat the MPC-based threat assessment provides a true assessment of theminimum instantaneous threat posed to the vehicle. In a driver warningcontext, threshold threat values may trigger driver warnings atcritical/desired threat thresholds. For semi-autonomous control viacomputer control, threat assessment may be used to determine when andhow strongly to intervene. The latter application is a topic of theSemi-Autonomous applications. This disclosure focuses on the design anddevelopment of the threat assessment metric itself.

A corridor-based trajectory-planning method may be used, based onconstrained optimal control. When the objective function and constraintsare defined as described below, the vehicle path calculated at each timestep by the MPC controller is assumed to be the best case or safest pathwithin a corridor through the environment. Some key metrics from thisprediction may be used to assess the instantaneous threat posed to thevehicle.

For a discrete plant model described byx _(k+1) =Ax _(k) +B _(u) u _(k) +B _(v) v _(k)y _(k) =Cx _(k) +D _(v) v _(k)with x, y, u, and v representing states, outputs, inputs, anddisturbances of the system respectively, a quadratic objective functionover a prediction horizon of p sampling intervals is defined as

$J_{k} = {{\sum\limits_{i = {k + 1}}^{k + p}{\frac{1}{2}\left( {y_{i} - r_{i}} \right)^{T}{R_{y}\left( {y_{i} - r_{i}} \right)}}} + {\sum\limits_{i = k}^{k + p - 1}{\frac{1}{2}u_{i}^{T}R_{u}u_{i}}} + {\sum\limits_{i = k}^{k + p - 1}{\frac{1}{2}\Delta\; u_{i}^{T}R_{\Delta\; u}\Delta\; u_{i}}} + {\frac{1}{2}\rho_{ɛ}ɛ^{2}}}$where R_(y), R_(u), and R_(Δu) represent diagonal weighting matricespenalizing deviations from y_(i)=r_(i), and u_(i)=0, ρ_(ε) representsthe penalty on constraint violations and ε represents the maximumconstraint violation over the prediction horizon p. Inequalityconstraints are defined as:y ^(j) _(min)(i)−εV ^(j) _(min)(i)≦y ^(j)(k+i+1|k)≦y ^(j) _(max)(i)+εV^(j) _(max)(i)u ^(j) _(min)(i)≦u ^(j)(k+i+1|k)≦u ^(j) _(max)(i)Δu ^(j) _(min)(i)≦Δu ^(j)(k+i+1|k)≦Δu ^(j) _(max)(i)i=0, . . . , p−1ε≧0where the vector Δu represents the change in input from one samplinginstant to the next, the superscript “(•)^(j)” represents the jthcomponent of a vector, k represents the current time, and the notation(•)j(k+i|k) denotes the value predicted for time k+i based on theinformation available at time k. The vector V allows for variableconstraint softening over the prediction horizon, p, when ε is includedin the objective function.

For reference trajectory tracking along a geometrically simple path,lateral deviation of the vehicle's center of gravity (y_(y)) from thecorridor centerline (r_(y)) is penalized by including R_(y)>0 in anobjective function of the form:

$J_{k} = {{\sum\limits_{i = {k + 1}}^{k + p}{\frac{1}{2}\left( {y_{i} - r_{i}} \right)^{T}{R_{y}\left( {y_{i} - r_{i}} \right)}}} + {\sum\limits_{i = k}^{k + p - 1}{\frac{1}{2}u_{i}^{T}R_{u}u_{i}}} + {\sum\limits_{i = k}^{k + p - 1}{\frac{1}{2}\Delta\; u_{i}^{T}R_{\Delta\; u}\Delta\; u_{i}}}}$where, k represents the current moment in time, p represents the numberof time steps in the prediction horizon, R_(y), R_(u), and R_(Δu)represent weighting matrices on the lateral position state (y), inputs(u), and input rates (Δu). The resulting trajectory-tracking setup of aprior art controller and threat assessor, through a hazard-containingenvironment may then be illustrated by FIG. 9 a.

FIG. 9 a shows a vehicle 902 a that seeks to avoid an obstacle ofanother vehicle 904 a and a pedestrian 906 a. A path based controllerand threat assessor would attempt to follow a desired single track pathy_(des). This path may safely navigate the hazards, but it isunnecessarily restrictive.

For corridor-keeping, penalties on deviation from a desired trajectory(R_(y)) are replaced with lateral position constraints. This form ofcorridor-based navigation assumes that the environment has beendelineated, with the boundaries of the navigable road surface at eachtime step described by the constraint vectors

${{y_{\max}^{y}(k)} = \begin{bmatrix}{y_{\max}^{y}\left( {k + 1} \right)} \\\vdots \\{y_{\max}^{y}\left( {k + p} \right)}\end{bmatrix}},{{y_{\min}^{y}(k)} = {\begin{bmatrix}{y_{\min}^{y}\left( {k + 1} \right)} \\\vdots \\{y_{\min}^{y}\left( {k + p} \right)}\end{bmatrix}.}}$In the preceding expression, y^(y) _(max) and y^(y) _(min) represent theupper and lower limits on the vehicle lateral position (y) asillustrated in FIG. 9 b. The driveable corridor 908 b is between theselimiting curves. These limits exclude more than simplyoff-road/out-of-lane regions from the navigable corridor—they alsoextend to stationary and/or moving hazards in the roadway such asdebris, pedestrians 906 b or other vehicles 904 b. Thus, a hazard in theroadway looks to the controller like a constriction in the corridor asillustrated by the arrows C in FIG. 9 b. The host vehicle 902 b ispermitted to travel anywhere within the corridor 908 b.

For the constraint space to remain feasibley ^(y) _(max) −y ^(y) _(min)>0

Constraints can be softened by including the magnitude of theirviolation ε in the objective function, which takes the form

${J_{k} = {{\sum\limits_{i = {k + 1}}^{k + p}{\frac{1}{2}y_{i}^{T}R_{y}y_{i}}} + {\sum\limits_{i = k}^{k + p - 1}{\frac{1}{2}u_{i}^{T}R_{u}u_{i}}} + {\sum\limits_{i = k}^{k + p - 1}{\frac{1}{2}\overset{¨}{A}u_{i}^{T}R_{\overset{¨}{A}u}\overset{¨}{A}u_{i}}} + {\frac{1}{2}\rho_{y}ɛ_{y}^{2}}}},\mspace{79mu}{R_{yy} = 0.}$

The MPC objective function can be configured to force the constrainedoptimal solutions to satisfy corridor constraints before minimizingfront wheel sideslip. This hierarchy of objectives is achieved bysetting constraint violation weights (ρ_(ε)) significantly higher thanthe competing minimization weight (R_(aa)) on front slip. Then whenconstraints are not active, front wheel sideslip—and the correspondingthreat—remains low. When the solution is constrained, predicted frontwheel sideslip increases with the severity of the maneuver required toremain within the navigable corridor.

Physical limits on tire cornering friction dictate maximum safe anglesof wheel sideslip. These angles provide an objective limit against whichpredicted sideslip may be normalized; when predicted threat approachesthis known limit, loss of stability is imminent. This inherentlimitation on stability-critical states such as front wheel slip makesthem particularly well suited as objective threat assessors.

Various norms may be used to reduce the vector of MPC predicted vehiclestates {right arrow over (x)} to a scalar threat metricΦ_({right arrow over (x)})(k) (instantaneous threat assessment at timek). The performance of several norms has been studied. Table 1 describeshow each was determined.

TABLE 1 Norms used to reduce MPC predicted states to a scalar threatmetric Φ{right arrow over (_(x))}(k) Symbol Description CalculationΦ{right arrow over (_(x))}⁰(k) First/nearest predicted x Φ{right arrowover (_(x))}⁰(k) = |x_(k+1)| Φ{right arrow over (_(x))}¹(k) Averagepredicted state {right arrow over (x)}${\Phi_{\overset{\rightharpoonup}{x}}^{1}(k)} = \left| \frac{\sum\limits_{i = 1}^{p}\;\left( x_{k + i} \right)}{p} \right|$Φ{right arrow over (_(x))}²(k) 2-Norm of predicted state {right arrowover (x)}${\Phi_{\overset{\rightharpoonup}{x}}^{2}(k)} = \sqrt{\frac{\sum\limits_{i = 1}^{p}\;\left( x_{k + i} \right)^{2}}{p^{2}}}$Φ{right arrow over (_(x))}^(∞)(k) Maximum predicted state {right arrowover (x)} Φ{right arrow over (_(x))}^(∞)(k) = max|{right arrow over(x)}| (Note that the superscripts in the Symbol column does notrepresent a power, but is part of the name, i.e, the first norm, the2^(nd) norm, the infinity norm.) Additional norms that may be usedinclude Root Mean Square of predicted state or states over theprediction horizon, and any of the above mentioned norms, with weightingprofiles, over the prediction horizon (i.e. state predictions at achosen time, for instance, closer to the current vehicle state may beweighted more heavily in the aggregate metric than predicted states atlater times or at times other than the specifically chosen time. Thechosen time may be other than the current time, such as a timeimmediately after some other event).

Just as various vehicle states may be penalized in the objectivefunction without significantly changing the MPC-generated trajectoryplan (as discussed above), these states may also be used somewhatinterchangeably to assess threat posed by a given trajectory prediction.Threat assessment based on lateral acceleration ({right arrow over(x)}={right arrow over (ÿ)}≡{right arrow over (a)}), front wheel slip({right arrow over (x)}={right arrow over (α)}), and a modifiedobjective function cost ({right arrow over (x)}={right arrow over(J)}_(SI)) have all been compared.

While lateral acceleration is used in existing threat metrics, frontwheel slip may also be used advantageously in this invention based onthree observations. First, front wheel slip is directly tied to, andtends to be a good indicator of, vehicle stability and controllabilityby front wheel steering. Second, available surface friction places ameasurable limit on how large front wheel slip angles can become beforeloss of control is imminent. This limit provides a useful benchmarkagainst which threat assessments can be compared to assess maneuverstability (or nearness to instability). Finally, when the costfunction's only state objective is to minimize front wheel slip (whileremaining within corridor- and actuator-imposed constraints), the pathprediction explicitly minimizes the very metric used to assess threat.This hierarchy of objectives—remain within the corridor while minimizingfront slip as much as possible—thereby provides a “best case” orminimal-threat assessment from a dynamically-feasible maneuver.

For some scenarios, however, the controller may not completely satisfyposition constraints, making a an incomplete indicator of the trueanticipated threat. These scenarios may arise when complex corridorscause constraints such as maximum input value or maximum input rate toactivate. In these situations, the MPC-predicted vehicle path mayviolate position constraints, making Φ_(a)=f(a_(predicted)) anincomplete threat assessment since it does not capture the additionalthreat posed by the predicted departure from the navigable corridor. Toaccount for such scenarios, an alternative threat metric similar to theobjective function cost may be used, where {right arrow over (x)}={rightarrow over (J)}_(SI), with {right arrow over (J)}_(SI) defined similarto the MPC objective function. By similar, it is meant of the same formand order. For example, the equation below shows what J_(SI) might looklike when the controller objective function penalizes wheel sideslip αby R_(α), steering angle δ by R_(δ), steering rate Δδ by R_(Δδ), andconstraint violation by ε:

${{\overset{\rightharpoonup}{J}}_{SI}(k)} = {{\frac{1}{2}{R_{\alpha}\begin{bmatrix}\alpha_{k + 1}^{2} \\\alpha_{k + 2}^{2} \\\vdots \\\alpha_{k + p}^{2}\end{bmatrix}}} + {\frac{1}{2}{R_{\delta}\begin{bmatrix}\delta_{k}^{2} \\\delta_{k + 1}^{2} \\\vdots \\\delta_{k + p - 1}^{2}\end{bmatrix}}} + {\frac{1}{2}{R_{\Delta\;\delta}\begin{bmatrix}{\Delta\delta}_{k}^{2} \\{\Delta\delta}_{k + 1}^{2} \\\vdots \\{\Delta\delta}_{k + p - 1}^{2}\end{bmatrix}}} + {\frac{1}{2}{{\rho_{SI}\begin{bmatrix}ɛ_{k + 1}^{2} \\ɛ_{k + 2}^{2} \\\vdots \\ɛ_{k + p}^{2}\end{bmatrix}}.}}}$This threat metric, while somewhat more difficult to interpretphysically, accounts for the additive presence of the various objectivefunction considerations, such as constraints, input costs, etc, andincreases rapidly when constraints are violated. This rate ofintervention is tuned independent of the controller cost function byintroducing a modified (and adjustable) constraint violation weight,ρ_(SI). The cost-based prediction {right arrow over (J)}_(SI) is relatedto the predicted front wheel sideslip by{right arrow over ({tilde over (J)}_({right arrow over (α)})=({rightarrow over (α)}−{right arrow over (r)} _({right arrow over (α)}))^(T) R_(αα)({right arrow over (α)}−{right arrow over (r)}_({right arrow over (α)})).With {right arrow over (r)}_(α)=0, this relation allows the cost-basedthreat assessment Φ_({right arrow over (J)}) _(SI) to be mapped to anequivalent (and physically-bounded) front-wheel-slip-based assessmentΦ_(J) via

$\Phi_{J} = {\sqrt{\frac{\Phi_{{\overset{\rightharpoonup}{J}}_{SI}}}{R_{\alpha\alpha}}}.}$

Simulation results conducted by the inventors hereof, but not reproducedherein, show how these prediction calculations, threat metrics, andprediction horizons affect the threat assessment. The simulations showthat the MPC-based threat assessment provided by the proposed frameworkgives a similar, albeit more situation- and plant-aware threatassessment to that based on a constant radius turn in simple(single-obstacle avoidance) scenarios. When generated using the maximumMPC-predicted lateral acceleration (∞-norm) in these simple scenarios,these predictions provide slightly earlier warning, which explicitlyaccounts for more complex hazard geometry and more realistic actuatorlimits. In more complicated/realistic scenarios where multiple hazardsare present, threat assessment provided by these inventions has beenshown to provide a very accurate assessment of the true threat (orexpected nearness to instability).

Assessing threat based on a controller-achievable maneuver requires somemeasure of how well the predicted threat/state represents what thevehicle would actually experience under autonomous control (truethreat). This relationship between predicted threat and the controller'sability to maintain true threat at or below this level plays animportant role in the semi-autonomous control implementation discussedin the Semi-Autonomous applications. Threat assessments using variouscontroller and prediction parameters have been compared by the inventorshereof to the true vehicle state and have been shown to provide areliable estimate of the vehicle's true states under autonomous control.

When hazard geometry is simple, the threat assessment generated bymethods disclosed herein closely mirrors and slightly precedes (givesmore time to prepare for than) a constant radius turn-based assessment.For more complex hazard avoidance scenarios, such as those requiring alane change maneuver, methods described herein are shown to account forthe increased threat, thus providing a significant improvement oversimple constant radius turn (CRT)-based assessments.

Methods disclosed herein have been shown in these simulations toaccurately predict an MPC-controlled vehicle's performance as it tracksthe MPC-predicted trajectory plan through a constrained corridor. Twometrics have been shown to provide a nearly one-to-one mapping ofpredicted threat to true threat, suggesting that, if provided fullcontrol of a vehicle, this MPC controller can reasonably be expected tomaintain critical vehicle states/threat at or below their predictedvalues. It is explained below why these inventions' threat assessmentmay be effectively used for both autonomous and semi-autonomousapplications. The inventors hereof have also conducted a comparison ofpredictions obtained using this objective function setup to objectivefunctions that penalizes lateral acceleration, vehicle roll angle, loadtransfer, and other states besides front wheel slip (R_(a) _(y) _(a)_(y) >0), with similarly favorable results.

It is also very beneficial that the threat assessment may changedepending on the circumstances, for example, if a car is travelingthrough an area corowded with pedestrians. Such a flexibility would bebased on a situational circumstance or severity. A crowded locationpresents a more severe situation than traveling along an open road.Thus, if there are many obstacles, or if there are several obstacles,but they are moving along complicated paths, the system may note thisand invoke a different level of threat assessment, because of thecomplexity, or severity of the situation.

One key to these inventions' effectiveness as both a threat assessor andsemi-autonomous controller is that it uses the same set of tools,calculations, and model-based predictions for assessing threat as itdoes for calculating inputs, and governing human and machine controlcommands. Rather than minimizing some arbitrary cost function or value,the trajectory predictor (and actuator command generator in the case ofsemi-autonomous assistance) minimizes the very characteristics thatdescribe threat. These characteristics, in turn, are based on anaccurate model of the vehicle whose performance, stability, and safetymay be assessed against an objective, physics-based metric.

In one particularly useful formulation, the threat assessor combinesstates, inputs, and constraint violations using a weighting function,that is of the same form and order as the weighting function used by thetrajectory planner and actuator command generator.

Extensions

Methods and apparati have been disclosed herein that assess threat inthe context of a system with a vehicle moving through an environment. Ashas been mentioned above, however, these methods and apparati of threatassessment can be used with any system that can be modeled. Forinstance, they can be used to assess threat in connection with anyhuman-automation system in which the performance of the system beingcontrolled may be reasonably modeled. By performance, it is meant thetrajectory of a machine, the motion, velocity or position of a tool,vehicle, etc. The methods and apparati may be used with vehicles of allsorts, including but not limited to terrestrial (ground), aerial,nautical, underwater, underground, etc. They can be used with heavyequipment, such as agricultural equipment, fork lift trucks, cranes,etc., which not only move from place to place in an environment, butalso, or alternatively, change their configuration from one moment tothe next, such as when a crane is extending its boom. They may be usedwith naval vessels. Additionally, they may be used with unmanned orremotely-operated vehicles such as unmanned aerial drones, unmannedground robots, etc.

The methods and apparati may be used with surgical machines, which areguided by a human surgeon who manipulates an input device, to drive anoutput device, such as a scalpel, cauterization tool, stitching machine,etc. The environment of the surgical tool may be considered to includethe body being operated upon, instrument supports, etc. They may also beused to control chemical processes, and power plants, such as nuclear,electric, etc.

The inventions disclosed herein are also useful with respect to devicesthat do not move from one point to another in an environment, and arealso useful in connection with large systems, such as power plants, andalso processes, such as chemical processes.

The terms: states, control inputs, operator inputs, trajectory, andregions, are all general to modeled systems and therefore translatedirectly to surgical devices, chemical processes, etc., as used hereinand in the accompanying claims.

A region of the state space bounded by state constraints is also genericto modeled systems. Corridor, as used herein, is a convenient way ofvisualizing a two-dimensional region and may therefore not be sointuitively useful in other applications. However, it may be meant touse a bounded two-dimensional corridor in a state space that isdifferent from a traditional, familiar two-dimensional physical space,such as that in which a vehicle travels. Thus, as used in the claims, itmeans a bounded two-dimensional space, analogous to a roadway interrestrial two-dimensional space.

A system that does not move within an environment still needs to accountfor the influence of external processes and disturbances upon it.Although these are not obstacles per se, they still represent threats tosystem success. In a chemical process, such an obstacle-like threatcould be an external agency that dumps chemicals at different intervalsinto the mixture under control, or an uncontrolled temperature variationin the environment of the plant. Regarding a chemical process, the termprocess plant is used herein and in the claims to refer to the volume ofmaterial that is being transformed and manipulated according to achemical process, which may be in a stationary vessel, or moving alongin a series of pipes and vessels, over time. The environment in whichthe process plant operates would include the vessels and atmospheres inwhich the process plant exists. Environmental disturbances, or hazards,could include ambient temperature influences, chemical pollutants,vibrations of equipment, etc. Rather than the locations of obstacles inphysical space being relevant to the control of a process, inventionsdisclosed herein use the coordinates of such influences, in whatevercoordinate system is relevant for understanding and evaluating theireffect upon the process under control. For instance, the coordinates ofchemical process hazards may relate to which vessel, or run of conduit adisturbance affects.

With respect to other systems, such as power plants, analogous factorswill be understood by those skilled in the art.

As has been mentioned, use of a threat assessment that is closely tiedto the means by which an optimal trajectory is established enables anoptimal framework of assisting a device operator. This frameworkexplicitly considers human and machine dynamics without presumingoperator intentions or limiting the avoidance maneuver (and itsassociated threat assessment) to a specific path. It provides a unifiedframework that allows for various modes and levels of operatorassistance, from operator warning to stability control to passiveintervention, to active semi-autonomous control, and finally, toautonomous machine operation. Autonomous operation may be local orremote.

Remotely operated systems typically exhibit a time lag. Because thismethod for threat assessment and semi-autonomous operator assistance isbased on a model predictive control prediction, it is particularly wellsuited for unmanned and remotely or ‘teleoperated’ systems such asUnmanned Ground Vehicles (UGV's), Unmanned Aerial Vehicles (UAV's),Unmanned Underwater Vehicles (UUV's), or remote surgical equipment. Inrelaying signals between the operator and the vehicle, such systemsfrequently experience communication latencies and time delays. Theselatencies and time delays can cause problems for prior art systemsseeking to assist the human driver. Such systems often requireadditional modules to mitigate the effects of latency and time delays.In contrast, the framework described here may be configured toexplicitly incorporate these time delays into the optimal trajectoryprediction, thereby eliminating the need for additional modules.

Operator Assistance

Known systems may be illustrated with reference to FIG. 10. Knownsystems use distinct and sometimes competing modules to assist anoperator. Such distinct, independent systems 1000 can include, but arenot limited to: warning devices 1002, a yaw stability controller 1004; aroll stability controller 1006, a traction controller 1008, a cruisecontroller 1010 and a lane-assist controller 1012. All of which may workindependently, and their combined output, (some of which are inputs toactuators that control the vehicle) can lead to inconsistent,unpredictable, unintended or suboptimal outcome 1014. For instance, theroll stability controller may output a steering command that isinconsistent with a steering command generated by the lane assistcontroller.

In contrast, as shown in FIG. 11, the inventions described here mayoperate as a unitary, consistent whole, 1100, in conjunction with, orindependent of, the human operator.

The MPC constitutes a state trajectory and control input planner 1102,which generates 1102 an optimal trajectory for the device, from currentposition through a time horizon, together with the device control inputsnecessary to follow that trajectory.

In a passive mode, these inventions constitute a physically-accurate,predictive and flexible means of predicting the threat posed to thevehicle given the driver's current and past performance, current andpast vehicle state evolution, and environmental features/constraints. Itis physically accurate because it is based on a forward-simulatedvehicle model. It is predictive, because the vehicle model is simulatedover a future time horizon. It is flexible because threat andintervention decisions are based on a corridor—rather than pathcharacteristics.

Warning and operator feedback systems 1104 may inform any of a number ofoperator warning devices, including audible, visual, haptic, olfactory,etc.

In addition to providing a predictive, flexible, and accurate passivewarning mechanism 1104, the inventions may be configured to predictivelyand semi-autonomously act as a device stability controller 1106.Examples of stability controllers that this system might replaceinclude, but are not limited to yaw stability controllers, rollstability controllers, anti-lock brakes and traction controls. Usingvarious mutually compatible actuation modes, including, for example,differential braking and steering adjustments, this framework predictsvehicle state evolution over a projected time horizon and adjusts thecurrent vehicle state to keep both current and predictedstability-critical states (such as yaw angle, wheel slip angles, etc.)below critical levels.

Another related intervention mode that these inventions are capable ofis passive driver assistance 1108. These subsystems can apply variousdegrees of resistance, such as to steering wheel resistance torqueoverlays, braking (anti-lock brakes) traction control, yaw stabilitycontrol, acceleration inhibitors, to discourage the operator fromfurther increasing threat. (Note that there is some overlap betweenpassive and reactive assistance systems and stability controllers.)

Active intervention modes 1110 go beyond providing resistive feedbackand actively initiate steering and/or braking commands.

The inventions are well-suited to semi-autonomously assist the driver;utilizing sensory information related to the vehicle surroundings and aprediction of a safe vehicle trajectory through those surroundings toexert appropriate actuator effort and avoid impending accidents. Sensoryinformation would include data related to nearby vehicles, pedestrians,road edges, and other salient features to assess accident threat. Asecond key advantage is that the invention may be configured to operateonly during instances of significant threat: giving the driver fullcontrol of the vehicle in low threat situations but applying appropriatelevels of computer-controlled actuator effort during high threatsituations.

The corridor-based threat assessment upon which this intervention isbased presents a key advantage over existing semi-autonomous safetysystems in that using a corridor (as opposed to a path) allowsintervention that does not unnecessarily constrain the human operator toa specific (and rather arbitrary) path.

As a semi-autonomous controller, the inventions allow forhuman-controlled, computer-controlled, and shared human/computer vehicleoperation. The form of the intervention law modulating control systemauthority allocation is configurable and can differ for differentactuators (i.e. a vehicle with both active steering and braking can havedistinct intervention laws defined for the steering actuator and thebraking actuators). The intervention law can also be defined to adapt todriver performance based on an assessment of driver skill, and/or toinclude considerations for driver preference, environmental conditions,previous threat metric values, previous control inputs, and otherfactors. The intervention law can also take on dynamics of its own,exhibiting, for example, hysteretic and higher-order behavior as afunction of past, current, and predictive threat. See FIG. 1.

Finally, the inventions are capable of fully-autonomous vehicle control.That is, in high-threat scenarios or when desired by the human operator,this system can take full control of the vehicle, safely navigating itthrough the environment while avoiding both collisions and loss ofcontrol.

This multiplicity of highly competent options enables a unitary,integrated, mutually consistent, optimal operator assistance system.Because the modules for effectuating each degree of autonomy inoperation described above are each based on the same optimal controlsequence of states and inputs, and are all tied together by virtue ofthe threat assessment apparatus and method that is also based on thesame sequence of states and inputs, the optimal set of inputs is optimaland internally consistent. The passive assistance portions of the systemwill not generate conflicting actuator inputs with other passiveassistance portions, or with the active assistance portions. Thus, thesystem can allow and direct control to flow seamlessly from a merewarning, to passive assistance, stability control, active assistance,semi-autonomous and fully autonomous, and back again, over and overagain, automatically. All of the threat response activities are mutuallyconsistent. This permits a truly, reliably user configurablesemi-autonomous system. All of this flexibility flows from the use ofgenerating an appropriate and active control input based on predictedthreat.

This unification through the optimal trajectory of position, control andstate sequences is highly significant. It means that one system canperform equally well the duties currently assigned to many disparatesystems. For example, a vehicle that was previously equipped withseparate driver warning, traction control, and yaw stability controlsystems (whose assistance can often conflict), may, with the presentinventions, perform each of these functions (and more still) with asingle, unified, and in some sense optimal framework. Thus, that asingle system reliably, elegantly, and highly satisfactorily performsany of these tasks individually, and all of the tasks together, whichotherwise would require multiple systems is significant. Such aninvention is far more than simply the sum of its parts.

It will be understood that the MPC path and trajectory planner 1102 andthe threat assessor 1103 and actuator controllers 1104, 1106, 1108, 1110may be microprocessor based such as a computer having a centralprocessing unit, memory (RAM and/or ROM), and associated input andoutput buses. They may be application specific integrated circuits ormay be formed of any other logic devices known in the art. They may be aportion of a central vehicle main control unit, an interactive vehicledynamics module, a main safety controller, or may be stand=-alonecontrollers. They may each have associated memories, may share a centralmemory or some combination of both. These controllers may performvarious different sensing system operations. The operations may beperformed sequentially or simultaneously. The controller and threatassessor may have a driver input.

The foregoing has described the computer systems that conduct methodsteps discussed herein relatively generically. It will be understood bythose skilled in the art that each of the processes described herein canbe performed by a dedicated specialized processor, or by a properlyprogrammed general purpose digital computer, or by some computing devicethat is more or less complex and specialized than either of theforegoing. As such, where in the claims it is stated that a processorperforms a specific function, it will be understood that such aprocessor may perform only the function stated therein, or additional,and even all processes mentioned within the claim. Assignment of suchprocesses to one or more processing devices is routine and would beconducted by a skilled person according to processing speed, weight.

This disclosure describes and discloses more than one invention. Theinventions are set forth in the claims of this and related documents,not only as filed, but also as developed during prosecution of anypatent application based on this disclosure. The inventors intend toclaim all of the various inventions to the limits permitted by the priorart, as it is subsequently determined to be. No feature described hereinis essential to each invention disclosed herein. Thus, the inventorsintend that no features described herein, but not claimed in anyparticular claim of any patent based on this disclosure, should beincorporated into any such claim.

Some assemblies of hardware, or groups of steps, are referred to hereinas an invention. However, this is not an admission that any suchassemblies or groups are necessarily patentably distinct inventions,particularly as contemplated by laws and regulations regarding thenumber of inventions that will be examined in one patent application, orunity of invention. It is intended to be a short way of saying anembodiment of an invention.

An abstract is submitted herewith. It is emphasized that this abstractis being provided to comply with the rule requiring an abstract thatwill allow examiners and other searchers to quickly ascertain thesubject matter of the technical disclosure. It is submitted with theunderstanding that it will not be used to interpret or limit the scopeor meaning of the claims, as promised by the Patent Office's rule.

The foregoing discussion should be understood as illustrative and shouldnot be considered to be limiting in any sense. While the inventions havebeen particularly shown and described with references to preferredembodiments thereof, it will be understood by those skilled in the artthat various changes in form and details may be made therein withoutdeparting from the spirit and scope of the inventions as defined by theclaims.

The corresponding structures, materials, acts and equivalents of allmeans or step plus function elements in the claims below are intended toinclude any structure, material, or acts for performing the functions incombination with other claimed elements as specifically claimed.

Having described the invention, what is claimed is:
 1. A method forgenerating a signal that represents an assessment of threat to amechanical device, operating in an environment, the method comprisingthe steps of: a. using a computer control system, using a model of theenvironment and a model of the device, generating at least one optimaldevice state trajectory from a current state through a time horizonsatisfying constraints imposed by device control inputs, devicedynamics, and the position of environmental hazards; b. using thecomputer control system, based on at least one generated optimaltrajectory prediction, identifying and quantifying threat posed to thedevice and generating a corresponding threat assessment metric, the atleast one generated optimal trajectory prediction being based at leaston predicted stability of the device through the time horizon as afunction of the at least one optimal device state trajectory; and c.using the computer control system, based on the threat assessment metricand at least one threat threshold relating to at least one of known,approximated, and desired stability limits and safety margins of thedevice, generating at least one threat assessment signal.
 2. The methodof claim 1, the time horizon comprising an infinite horizon.
 3. Themethod of claim 1, the time horizon comprising a finite horizon.
 4. Themethod of claim 1, the model of the environment comprising a changeablemodel.
 5. The method of claim 1, the model of the device comprising anadaptable model.
 6. The method of claim 1, the model of the devicetaking into account non-linear dynamics.
 7. The method of claim 1,further comprising the step of, using the computer control system, basedon the threat assessment signal, generating a humanly perceptible threatassessment signal.
 8. The method of claim 7, the humanly perceptiblesignal comprising a signal selected from the group consisting of: anaudible signal, a visual signal, a haptic signal, and an olfactorysignal.
 9. The method of claim 8, the humanly perceptible signal varyingin at least one of frequency and amplitude based on the threatassessment metric.
 10. The method of claim 1, a. wherein: i. the modelof the environment comprises a constraint bounded corridor passingthrough the environment and containing in its interior none of thefollowing: A. current location of any obstacle; and B. current locationof any environmental hazard; and ii. the model of the device comprisesat least one aspect selected from the group consisting of: A. a devicedynamic constraint; B. a device input constraint; and C. a devicevariable state; b. the method further comprising the steps of using thecomputer control system: i. using the model of the environment and themodel of the device, generating constraints on at least one current andpredicted device state in an n-dimensional space, where n>0, to generatean n-dimensional region of operability; ii. conducting at least one stepselected from the group consisting of: A. generating a sequence ofpredicted device states that are optimal with respect to an objectivefunction and satisfy input and state constraints; and B. generating asequence of device inputs that, when applied to a model of the device,generate a sequence of predicted states that is optimal with respect toan objective function and satisfies input and state constraints; iii.based on at least one predicted state from the sequence of optimalstates, generating the aggregate threat assessment metric; and iv.comparing the aggregate threat assessment metric to the predeterminedthreat thresholds and based on the comparison, generating the threatassessment signal.
 11. The method of claim 10, further the constraintbounded corridor containing in its interior none of the following: a. apredicted location of an obstacle; and b. a predicted location of anenvironmental hazards.
 12. The method of claim 10, the environmentalhazard comprising at least one selected from the group consisting of:terrain features, lane boundaries, road edges, static obstacles, andmoving obstacles.
 13. The method of claim 10, the device comprising aground vehicle, the device dynamic and input constraints being selectedfrom the group consisting of: vehicle position maximum steer angle,minimum steer angle, maximum steer rate, minimum steer rate, maximumacceleration due to braking, minimum acceleration due to braking,maximum acceleration torque, minimum acceleration torque, maximum wheelsideslip angle, maximum tire cornering stiffness and maximum roll angle.14. The method of claim 10, the step of generating the aggregate threatassessment metric being based on using all of the predicted states ofthe sequence of optimal states.
 15. The method of claim 10, the step ofgenerating the aggregate threat assessment metric being based on atleast one of current and past driver performance.
 16. The method ofclaim 10, the step of generating the aggregate threat assessment metricbeing based on at least one aspect selected from the group consistingof: situational complexity, situational severity, situationalcircumstance, risk of collision, risk of instability, and risk of lossof control.
 17. The method of claim 10, the device comprising a groundvehicle, the states being selected from the group consisting of: lateralacceleration, longitudinal wheel slip, wheel sideslip angle, vehiclesideslip angle, vehicle roll angle, lateral load transfer, longitudinalload transfer, and vehicle yaw rate.
 18. The method of claim 10, thestep of generating the aggregate threat assessment metric comprisingbasing the generation on a maximum predicted value of at least one stateover a prediction horizon.
 19. The method of claim 18, the step ofbasing the generation on a maximum predicted value comprising weightingstate predictions that are closer in time to a vehicle state at a chosentime more heavily in the aggregate threat metric than predicted statesat different times.
 20. The method of claim 10, the step of generatingthe aggregate threat assessment metric comprising basing the generationon an average predicted value of at least one state over a predictionhorizon.
 21. The method of claim 20, the step of basing the generationon an average predicted value comprising weighting state predictionsthat are closer in time to a vehicle state at a chosen time more heavilyin the aggregate threat metric than predicted states at different times.22. The method of claim 10, the step of generating the aggregate threatassessment metric comprising basing the generation on a Root Mean Square(RMS) of predicted value of at least one state over a predictionhorizon.
 23. The method of claim 22, the step of basing the generationon a Root Mean Square of predicted value comprising weighting statepredictions that are closer in time to a vehicle state at a chosen timemore heavily in the aggregate threat metric than predicted states atdifferent times.
 24. The method of claim 10, the step of generating theaggregate threat assessment metric comprising basing the generation on amaximum predicted value of at least one device control input over aprediction horizon.
 25. The method of claim 24, the step of basing thegeneration on a maximum predicted value comprising weighting inputpredictions that are closer in time to a vehicle input at a chosen timemore heavily in the aggregate threat metric than predicted states atdifferent times.
 26. The method of claim 10, the step of generating theaggregate threat assessment metric comprising basing the generation onan average predicted value of at least one device control input over aprediction horizon.
 27. The method of claim 26, the step of basing thegeneration on an average predicted value comprising weighting devicecontrol input predictions that are closer in time to a vehicle input ata chosen time more heavily in the aggregate threat metric than predictedinputs at different times.
 28. The method of claim 10, the step ofgenerating the aggregate threat assessment metric comprising basing thegeneration on a Root Mean Square (RMS) of predicted value of at leastone device control input over a prediction horizon.
 29. The method ofclaim 28, the step of basing the generation on a Root Mean Square ofpredicted value comprising weighting device control input predictionsthat are closer in time to a vehicle input at a chosen time more heavilyin the aggregate threat metric than predicted inputs at different times.30. The method of claim 10, the step of generating the aggregate threatassessment metric comprising determining a threat cost at eachprediction step in the optimal sequence using a weighting function ofthe same form as the objective function used to generate a sequence ofat least one optimal device state and optimal device inputs andcombining the sequence of determined threat costs to generate theaggregate threat assessment metric.
 31. The method of claim 30, the stepof generating the aggregate threat assessment metric comprising basingthe generation on at least one norm, selected from the group consistingof: a. a maximum predicted value of at least one state over a predictionhorizon; b. an average predicted value of at least one state over aprediction horizon c. a Root Mean Square (RMS) of predicted value of atleast one state over a prediction horizon; d. a maximum predicted valueof at least one input over a prediction horizon; e. an average predictedvalue of at least one input over a prediction horizon f. a Root MeanSquare (RMS) of predicted value of at least one input over a predictionhorizon; and g. a weighted prediction of any of the factors of statesand inputs listed in sub-paragraphs a-f of this claim, formed byweighting predictions that are closer in time to a device factor at achosen time more heavily in the aggregate threat metric than a predictedfactor at different times.
 32. The method of claim 10, the devicecomprising a ground vehicle, the predetermined threat thresholds beingselected from the group consisting of: maximum sideslip angle beforeloss of control, maximum load transfer before wheel liftoff, maximumlateral acceleration before skidding, maximum longitudinal accelerationbefore skidding, maximum total acceleration before skidding, maximumsteer angle before actuator saturation, maximum available accelerationtorque, and maximum available braking torque.
 33. The method of claim10, further comprising the step of, using the computer control system,normalizing the threat assessment metric against a predetermined limit.34. The method of claim 33, the predetermined limit being selected fromthe group consisting of: a physical constraint, a dynamic constraint,and a preference.
 35. The method of claim 1, further comprising the stepof, using the computer control system, based on the threat assessmentmetric, generating a passive device control assistance.
 36. The methodof claim 35, the device comprising a ground vehicle, the passive devicecontrol assistance comprising at least one selected from the groupconsisting of: resistance torques on a steering input member; resistancetorques on a braking input member; resistance torques on an accelerationinput member; acceleration torques; braking torques; priming wheelbrakes, preparing safety devices and modifying device properties. 37.The method of claim 1, further comprising the step of, using thecomputer control system, based on the threat assessment metric,generating an active device control assistance.
 38. The method of claim37, the device comprising a ground vehicle, the active device controlassistance comprising at least one selected from the group consistingof: traction control, adaptive cruise control, yaw stability control,roll stability control, electronic stability control; lane keeping andobstacle avoidance through active steering control, lane keeping andobstacle avoidance through the imposition of breaking inputs; lanekeeping and obstacle avoidance through the imposition of acceleratorinputs.
 39. The method of claim 1, further comprising the step of, usingthe computer control system, based on the threat assessment metric,generating an active device modulation signal.
 40. The method of claim39, the vehicle comprising a ground vehicle, the device modulationsignal comprising: suspension modification.
 41. The method of claim 1,the device comprising a ground vehicle.
 42. The method of claim 1, thedevice comprising an aerial vehicle.
 43. The method of claim 1, thedevice comprising a naval vessel.
 44. The method of claim 1, the devicecomprising heavy construction equipment.
 45. The method of claim 1, thedevice comprising a fork lift truck.
 46. The method of claim 1, thedevice comprising a surgical device.
 47. The method of claim 1, thedevice comprising a teleoperated device residing in the environment,which device environment is distant from an environment in which a humandevice operator resides.
 48. A threat assessment apparatus thatgenerates a signal that represents an assessment of threat to amechanical device, operating in an environment, the apparatuscomprising: a. a processor within which is represented a model of theenvironment and a model of the device, which processor is configured togenerate at least one optimal device state trajectory from a currentstate through a time horizon satisfying constraints imposed by devicecontroller inputs, device dynamics, and the position of environmentalhazards; b. a processor which is configured to, based on the at leastone generated optimal trajectory prediction, identify and quantifythreat posed to the device and to generate a corresponding threatassessment metric, the at least one generated optimal trajectoryprediction being based at least on predicted stability of the devicethrough the time horizon as a function of the at least one optimaldevice state trajectory; and c. a processor which is configured to,based on the threat assessment metric and at least one threat thresholdrelating to at least one of known, approximated, and desired stabilitylimits and safety margins of the device, generate at least one threatassessment signal.
 49. The apparatus of claim 48, further, a. wherein:i. the model of the environment comprises a constraint bounded corridorthat excludes: A. current location of any obstacle; and B. currentlocation of any environmental hazard; and ii. the model of the devicecomprises at least one aspect selected from the group consisting of: A.a device dynamic constraint; B. a device input constraint; and C. adevice variable state; b. the apparatus further configured to: i. usingthe model of the environment and the model of the device, generateconstraints on at least one current and predicted device state in ann-dimensional space, where n>0, to generate an n-dimensional region ofoperability; ii. conduct at least one step selected from the groupconsisting of: A. generating a sequence of predicted device states thatare optimal with respect to an objective function and satisfy, input andstate constraints; B. generating a sequence of device inputs that, whenapplied to a model of the device, generate a sequence of predictedstates that is optimal with respect to an objective function andsatisfies input and state constraints; iii. based on at least onepredicted state of the sequence of states, generate the aggregate threatassessment metric; and iv. compare the aggregate threat assessmentmetric to the predetermined threat thresholds and based on thecomparison, generate the threat assessment signal.
 50. The apparatus ofclaim 49, further the constraint bounded corridor excluding alllocations of at least one, selected from the group consisting of: a. apredicted location of an obstacle; and b. a predicted location of anenvironmental hazards.
 51. The apparatus of claim 49, the step ofgenerating the aggregate threat assessment metric being based on usingall of the predicted states of the sequence of optimal states.
 52. Theapparatus of claim 49, the step of generating the aggregate threatassessment metric comprising basing the generation on at least one norm,selected from the group consisting of: a. a maximum predicted value ofat least one state over a prediction horizon; b. an average predictedvalue of at least one state over a prediction horizon c. a Root MeanSquare (RMS) of predicted value of at least one state over a predictionhorizon; d. a maximum predicted value of at least one input over aprediction horizon; e. an average predicted value of at least one inputover a prediction horizon f. a Root Mean Square (RMS) of predicted valueof at least one input over a prediction horizon; and g. a weightedprediction of any of the factors of states and inputs listed insub-paragraphs a-f of this claim, formed by weighting predictions thatare closer in time to a factor at a chosen time more heavily in theaggregate threat metric than predicted factor at different times. 53.The apparatus of claim 49, the step of generating the aggregate threatassessment metric comprising determining a threat cost at eachprediction step in the optimal sequence using a weighting function ofthe same form as the objective function used to generate a sequence ofat least one optimal device state and optimal device input and combiningthe sequence of determined threat costs to generate the aggregate threatassessment metric.
 54. The apparatus of claim 52, the step of generatingthe aggregate threat assessment metric comprising basing the generationon at least one norm, selected from the group consisting of: a. amaximum predicted value of at least one state over a prediction horizon;b. an average predicted value of at least one state over a predictionhorizon c. a Root Mean Square (RMS) of predicted value of at least onestate over a prediction horizon; d. a maximum predicted value of atleast one input over a prediction horizon; e. an average predicted valueof at least one input over a prediction horizon f. a Root Mean Square(RMS) of predicted value of at least one input over a predictionhorizon; and g. a weighted prediction of any of the factors of statesand inputs listed in sub-paragraphs a-f of this claim, formed byweighting predictions that are closer in time to a factor at a chosentime more heavily in the aggregate threat metric than a predicted factorat different times.